% Devin Koepl

classdef ActuatedModelClass < handle
   
    properties (Constant) 
        %------------------------------------------------------------------
        
        % Simulation parameters
        
        % Body mass
        m   = 24;
        
        % Moments of inertia.  The reflected leg inertia is Il / rl^2.
        Ib  = 1;
        Ih  = 1;
        Il  = 1;
        
        % Robot stiffnesses
        kh  = 0; %1e3;
        kl  = 3e3;
        
        % Robot transmission
        G   = 20;
        rl  = 0.05;
        
        % Gravitational acceleration
        g   = 9.81;
        
        % Preferred zero force leg length
        r0 = 0.9;
        
        %------------------------------------------------------------------
        
        % Index variables.
        
        % CoM position indices
        xci     = 1;
        dxci    = 2;
        yci     = 3;
        dyci    = 4;
        
        % Angle (theta) indices
        obi     = 5;
        dobi    = 6;
        ohi     = 7;
        dohi    = 8;
        oli     = 9;
        doli    = 10;
        
        % Toe position indices
        xti     = 11;
        yti     = 12;
        
        % Event indices
        LO      = 1;
        TD      = 2;
        fall    = 3;
        
        %------------------------------------------------------------------
    end

    properties
        % Event variables
        te;
        xe;
        ie;

        % TD position
        xtd;
        ytd;
        
        % Ground properties
        kx;
        ky;
        bx;
        by;
        
        % State cell arrays
        options;
        state_fun;
        
        stance_dynamics;
        
        gnd;
        
        controller;
        
        dxtdyt;
    end
    
    methods (Static)
        
         function leg_length = R(x)
           if ( size(x, 1) == ActuatedModelClass.doli ) 
               % In flight so return zero force leg length (rz).
               leg_length = ActuatedModelClass.RZ(x);
           else
               % In stance so return distance from toe to CoM.
               leg_length = sqrt( ( x(ActuatedModelClass.xci, :) - x(ActuatedModelClass.xti, :) ).^2 ...
                   + ( x(ActuatedModelClass.yci, :) - x(ActuatedModelClass.yti, :) ).^2 );
           end
        end
        
        function leg_angle = PSI(x)
            if ( size(x, 1) == ActuatedModelClass.doli ) 
                % In flight so return zero force leg angle.
                leg_angle = x(ActuatedModelClass.ohi, :);
            else
                % If the leg is shorter than 1mm, assume that it is aligned
                % with the hip.
                if ( ActuatedModelClass.R(x) < 1e-3 )
                    leg_angle = x(ActuatedModelClass.ohi, :);
                else
                    % In stance so return angle to toe mass vector.
                    leg_angle = atan2( x(ActuatedModelClass.yci, :) - x(ActuatedModelClass.yti, :), ...
                        x(ActuatedModelClass.xci, :) - x(ActuatedModelClass.xti, :) );
                end
            end            
        end
        
        function zero_force_leg_length = RZ(x)
            zero_force_leg_length = ActuatedModelClass.rl ...
                * x(ActuatedModelClass.oli, :) / ActuatedModelClass.G;
        end
        
    end       

    methods

        function obj = ActuatedModelClass(x0, ground_function, controller)
            obj.te = 0;
            obj.xe = x0;
            obj.ie = 1;

            obj.options = { odeset( 'Events', @obj.flight_events, 'refine', 20, 'reltol', 1e-9 ),...
                odeset( 'Events', @obj.stance_events, 'refine', 20, 'reltol', 1e-9 ) };
            obj.state_fun = {@obj.flight_dynamics, obj.stance_dynamics};
            
            obj.gnd = GroundClass(ground_function);
            
            obj.controller = controller;
        end

        function [t, x] = run(obj)            
            switch ( obj.ie )
                case obj.LO
                    % Flight phase.
                    fprintf('Flight!\n');
                    
                    % Notify the controller of liftoff.
                    obj.controller.lo( obj.te, obj.xe' );
                    
                    [t, x, obj.te, obj.xe, obj.ie] = ode113(@obj.flight_dynamics, ...
                        [obj.te, 30], obj.xe(1 : obj.doli), obj.options{obj.ie});
                    
                    rz  = obj.RZ( x' )';
                    psi = obj.PSI( x' )';
                    x   = [ x, x(:, obj.xci) - rz .* cos(psi), ...
                        x(:, obj.yci) - rz .* sin(psi) ];
                      
                    obj.te = obj.te(end);
                    obj.ie = obj.ie(end);
                    obj.xe = obj.xe(end, :);   

                    rz  = obj.RZ( obj.xe' );
                    psi = obj.PSI( obj.xe' );

                    obj.xe   = [ obj.xe, obj.xe(obj.xci) - rz .* cos(psi), ...
                        obj.xe(obj.yci) - rz .* sin(psi) ];  
                                        
                case obj.TD
                    % Stance phase.  Choose the appropriate stance dynamics.
                    fprintf('Stance!\n');
                    
                    obj.dxtdyt = [ 0; 0 ];
                    
                    % Notify the controller of touchdown.
                    obj.controller.td( obj.te, obj.xe' );
                                        
                    obj.xtd = obj.xe( obj.xti );
                    obj.ytd = obj.xe( obj.yti );
                    
                    obj.kx = obj.gnd.kx( obj.xtd );
                    obj.ky = obj.gnd.ky( obj.xtd );
                    obj.bx = obj.gnd.bx( obj.xtd );
                    obj.by = obj.gnd.by( obj.xtd );
                    
                    obj.gnd.new_fp(0, obj.te, obj.xtd, obj.ytd, 0, 0);
                    
                    if ( ( isinf(obj.kx) || isinf(obj.bx) ) && ( isinf(obj.ky) || isinf(obj.by) ) )
                        obj.stance_dynamics = @obj.rigid_ground;
                    elseif ( (obj.kx < inf) && (obj.bx > 0) && (obj.bx < inf) ...
                            && (obj.ky < inf) && (obj.bx > 0) && (obj.by < inf) )
                        obj.stance_dynamics = @obj.compliant_ground;
                    elseif ( (obj.bx == 0) && (obj.by == 0) )
%                         springy = 1;
                        obj.stance_dynamics = @obj.springy_ground;
                    elseif ( ( isinf(obj.kx) || isinf(obj.bx) ) && (obj.by > 0) )
                        obj.stance_dynamics = @obj.xrigid_ycompliant_ground;
                    elseif ( (obj.bx > 0) && ( isinf(obj.ky) || isinf(obj.by) ) )
                        obj.stance_dynamics = @obj.xcompliant_yrigid_ground;
                    elseif ( ( isinf(obj.kx) || isinf(obj.bx) ) && (obj.by == 0) )
                        obj.stance_dynamics = @obj.xrigid_yspringy_ground;
                    elseif ( ( obj.bx == 0 ) && ( isinf(obj.ky) || isinf(obj.by) ) )
                        obj.stance_dynamics = @obj.xspringy_ycompliant_ground;
                    elseif ( (obj.bx > 0) && (obj.by == 0) )
                        obj.stance_dynamics = @obj.xcompliant_yspringy_ground;
                    elseif ( (obj.bx == 0) && (obj.by > 0) )
                        obj.stance_dynamics = @obj.xspringy_ycompliant_ground;
                    end   
                    
                    [t, x, obj.te, obj.xe, obj.ie] = ode113(obj.stance_dynamics, ...
                        [obj.te, 30], obj.xe, obj.options{obj.ie});              
                    
                    obj.te = obj.te(end);
                    obj.ie = obj.ie(end);
                    obj.xe = obj.xe(end, :);
                    
                    obj.gnd.set_lo(obj.te, obj.xe(ActuatedModelClass.xti), ...
                        obj.xe(ActuatedModelClass.yti));
                    
%                     figure;
%                     yd = obj.controller.slip_model.state(t - t(1));                     
%                     plot( t, [ x(:, obj.ohi), yd(:, SlipModelClass.oi) ] );
                    
                otherwise
                    % The model already fell, so do nothing.                            
                    t = obj.te;
                    x = obj.xe;
            end
            
        end

        %------------------------------------------------------------------
        
        % Flight
        
        function xdot = flight_dynamics(obj, t, x)
            u = obj.controller.flight(t, x);
            
            xdot = [x(obj.dxci)                         ;
                    0                                   ;
                    x(obj.dyci)                         ;
                    -obj.g                              ;
                    x(obj.dobi)                         ;
                    -obj.G * u(ControllerClass.tauhi) / obj.Ib      ;
                    x(obj.dohi)                         ;
                    obj.G * u(ControllerClass.tauhi) / obj.Ih       ;
                    x(obj.doli)                         ;
                    obj.G * u(ControllerClass.tauli) / obj.Il        ];
        end
 
        function [value, isterminal, direction] = flight_events(obj, t, x)
            rz      = obj.RZ(x);
            psi     = obj.PSI(x);
            
            xt      = x(obj.xci) - rz * cos(psi);
            yt      = x(obj.yci) - rz * sin(psi);
            
            if ( x( obj.dyci ) > 0 )
                TD_event = 1;
            else
                TD_event =  yt - obj.gnd.ground_height(t, xt, 0, inf);
            end
            
            value       = [1 ; TD_event ; x(obj.yci) - obj.gnd.ground_height(t, x(obj.xci), 0, inf) ];     % detect height = 0
            isterminal  = [0 ; 1 ; 1 ];   % stop the integration
            direction   = [0 ; -1 ; -1 ];   % negative direction
        end
        
        %------------------------------------------------------------------
        
        % Stance
        
        function xdot = rigid_ground(obj, t, x)
            r   = obj.R(x);
            psi = obj.PSI(x);
            
            % Toe velocity
            obj.dxtdyt = [ 0; 0 ];
            
            u = obj.controller.stance( t, x );
            
            xdot = [x(obj.dxci);
                    obj.kl * obj.rl * x(obj.oli) * cos(psi) / obj.G / obj.m...
                        - r * obj.kl * cos(psi) / obj.m...
                        + obj.kh * x(obj.ohi) * sin(psi) / obj.m / r...
                        - obj.kh * psi * sin(psi) / obj.m / r;
                    x(obj.dyci);
                    obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.G / obj.m...
                        - obj.kl * r * sin(psi) / obj.m...
                        + obj.kh * x(obj.ohi) * cos(psi) / obj.m / r...
                        - obj.kh * psi * cos(psi) / obj.m / r...
                        - obj.g;
                    x(obj.dobi);
                    - obj.G * u(ControllerClass.tauhi) / obj.Ib;
                    x(obj.dohi);
                    obj.kh * psi / obj.Ih...
                        - obj.kh * x(obj.ohi) / obj.Ih...
                        + obj.G * u(ControllerClass.tauhi) / obj.Ih;
                    x(obj.doli);
                    - obj.kl * obj.rl^2 * x(obj.oli) / obj.G / obj.Il...
                        + r * obj.kl * obj.rl / obj.Il...
                        + obj.G * u(ControllerClass.tauli) / obj.Il;
                    0;
                    0;                                                      ];
        end
        
        function xdot = compliant_ground(obj, t, x)            
            r   = obj.R(x);
            psi = obj.PSI(x);
            
            obj.dxtdyt = [ obj.kx * obj.xtd / obj.bx...
                        - obj.kx * x(obj.xti) / obj.bx...
                        - obj.kl * obj.rl * x(obj.oli) * cos(psi) / obj.bx / obj.G...
                        + obj.kl * r * cos(psi) / obj.bx...
                        - obj.kh * x(obj.ohi) * cos(psi) / obj.bx / r...
                        + obj.kh * psi * cos(psi) / obj.bx / r;
                    obj.ky * obj.ytd / obj.by...
                        - obj.ky * x(obj.yti) / obj.by...
                        - obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.by / obj.G...
                        + obj.kl * r * sin(psi) / obj.by...
                        - obj.kh * x(obj.ohi) * cos(psi) / obj.by / r...
                        + obj.kh * psi * cos(psi) / obj.by / r                              ];  
            
            u = obj.controller.stance(t, x, obj.dxtdyt);
            
            xdot = [x(obj.dxci);
                    obj.kl * obj.rl * x(obj.oli) * cos(psi) / obj.G / obj.m...
                        - r * obj.kl * cos(psi) / obj.m...
                        + obj.kh * x(obj.ohi) * sin(psi) / obj.m / r...
                        - obj.kh * psi * sin(psi) / obj.m / r;
                    x(obj.dyci);
                    obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.G / obj.m...
                        - obj.kl * r * sin(psi) / obj.m...
                        + obj.kh * x(obj.ohi) * cos(psi) / obj.m / r...
                        - obj.kh * psi * cos(psi) / obj.m / r...
                        - obj.g;
                    x(obj.dobi);
                    - obj.G * u(ControllerClass.tauhi) / obj.Ib;
                    x(obj.dohi);
                    obj.kh * psi / obj.Ih...
                        - obj.kh * x(obj.ohi) / obj.Ih...
                        + obj.G * u(ControllerClass.tauhi) / obj.Ih;
                    x(obj.doli);
                    - obj.kl * obj.rl^2 * x(obj.oli) / obj.G / obj.Il...
                        + r * obj.kl * obj.rl / obj.Il...
                        + obj.G * u(ControllerClass.tauli) / obj.Il;
                    obj.dxtdyt                                               ];       
        end
        
        function xdot = springy_ground(obj, t, x)
            r   = obj.R(x);
            psi = obj.PSI(x);
            
            rdot = ( x(obj.dxci) - obj.dxtdyt(1) ) * cos(psi) ...
                + ( x(obj.dyci) - obj.dxtdyt(2) ) * sin(psi);
            psidot = ( x(obj.dyci) - obj.dxtdyt(2) ) * cos(psi) / r...
                - ( x(obj.dxci) - obj.dxtdyt(1) ) * sin(psi) / r;
                                    
            obj.dxtdyt = [ - obj.kl * obj.rl * x(obj.doli) * cos(psi) / obj.kx / obj.G...
                    + obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.kx / obj.G...
                    + obj.kl * rdot * cos(psi) / obj.kx...
                    - obj.kl * r * sin(psi) / obj.kx...
                    + obj.kh * x(obj.dohi) * r * sin(psi) / obj.kx / r^2 ...
                    - obj.kh * x(obj.ohi) * r * cos(psi) / obj.kx / r^2 ...
                    - obj.kh * x(obj.ohi) * rdot * sin(psi) / obj.kx / r^2 ...
                    - obj.kh * psidot * r * sin(psi) / obj.kx / r^2 ...
                    + obj.kh * psi * r * cos(psi) / obj.kx / r^2 ...
                    + obj.kh * psi * rdot * sin(psi) / obj.kx / r^2;
                - obj.kl * obj.rl * x(obj.doli) * sin(psi) / obj.ky / obj.G ...
                    - obj.kl * obj.rl * x(obj.oli) * cos(psi) / obj.ky / obj.G ...
                    + obj.kl * rdot * sin(psi) / obj.ky ...
                    + obj.kl * r * cos(psi) / obj.ky ...
                    - obj.kh * x(obj.dohi) * r * cos(psi) / obj.ky / r^2 ...
                    + obj.kh * x(obj.ohi) * r * sin(psi) / obj.ky / r^2 ...
                    + obj.kh * x(obj.ohi) * rdot * cos(psi) / obj.ky / r^2 ...
                    + obj.kh * psidot * r * cos(psi) / obj.ky / r^2 ...
                    - obj.kh * psi * r * sin(psi) / obj.ky / r^2 ...
                    - obj.kh * psi * rdot * cos(psi) / obj.ky / r^2                   ];
            
            u = obj.controller.stance(t, x, obj.dxtdyt);
            
            xdot = [x(obj.dxci);
                    obj.kl * obj.rl * x(obj.oli) * cos(psi) / obj.G / obj.m...
                        - r * obj.kl * cos(psi) / obj.m...
                        + obj.kh * x(obj.ohi) * sin(psi) / obj.m / r...
                        - obj.kh * psi * sin(psi) / obj.m / r;
                    x(obj.dyci);
                    obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.G / obj.m...
                        - obj.kl * r * sin(psi) / obj.m...
                        + obj.kh * x(obj.ohi) * cos(psi) / obj.m / r...
                        - obj.kh * psi * cos(psi) / obj.m / r...
                        - obj.g;
                    x(obj.dobi);
                    - obj.G * u(ControllerClass.tauhi) / obj.Ib;
                    x(obj.dohi);
                    obj.kh * psi / obj.Ih...
                        - obj.kh * x(obj.ohi) / obj.Ih...
                        + obj.G * u(ControllerClass.tauhi) / obj.Ih;
                    x(obj.doli);
                    - obj.kl * obj.rl^2 * x(obj.oli) / obj.G / obj.Il...
                        + r * obj.kl * obj.rl / obj.Il...
                        + obj.G * u(ControllerClass.tauli) / obj.Il;
                    obj.dxtdyt                                                      ];                 
        end
        
        function xdot = xrigid_ycompliant_ground(obj, t, x)
            r   = obj.R(x);
            psi = obj.PSI(x);
            
            u = [0; 0];
            
            xdot = [x(obj.dxci);
                    obj.kl * obj.rl * x(obj.oli) * cos(PSI) / obj.G / obj.m...
                        - r * obj.kL * cos(psi) / obj.m...
                        + obj.kh * x(obj.ohi) * sin(psi) / obj.m / r...
                        - obj.kh * psi * sin(psi) / obj.m / r;
                    x(obj.dyci);
                    obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.G / obj.m...
                        - obj.kl * r * sin(psi) / obj.m...
                        + obj.kh * x(obj.ohi) * cos(psi) / obj.m / r...
                        - obj.kh * psi * cos(psi) / obj.m / r...
                        - obj.g;
                    x(obj.dobi);
                    - obj.G * u(ControllerClass.tauhi) / obj.Ib;
                    x(obj.dohi);
                    obj.kh * psi / obj.Ih...
                        - obj.kh * x(obj.ohi) / obj.Ih...
                        + obj.G * u(ControllerClass.tauhi) / obj.Ih;
                    x(obj.doli);
                    - obj.kl * obj.rl^2 * x(obj.oli) / obj.G / obj.Il...
                        + r * obj.kl * obj.rl / obj.Il...
                        + obj.G * u(ControllerClass.tauli) / obj.Il;
                    0;
                    obj.ky * obj.ytd / obj.by...
                        - obj.ky * x(obj.yti) / obj.by...
                        - obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.G / obj.by...
                        + obj.kl * r * sin(psi) / obj.by...
                        - obj.kh * x(obj.ohi) * cos(psi) / obj.by / r...
                        + obj.kh * psi * cos(psi) / obj.by / r                              ];            
        end
        
        function xdot = xcompliant_yrigid_ground(obj, t, x)
            r   = obj.R(x);
            psi = obj.PSI(x);
            
            u = [0; 0];
            
            xdot = [x(obj.dxci);
                    obj.kl * obj.rl * x(obj.oli) * cos(PSI) / obj.G / obj.m...
                        - r * obj.kL * cos(psi) / obj.m...
                        + obj.kh * x(obj.ohi) * sin(psi) / obj.m / r...
                        - obj.kh * psi * sin(psi) / obj.m / r;
                    x(obj.dyci);
                    obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.G / obj.m...
                        - obj.kl * r * sin(psi) / obj.m...
                        + obj.kh * x(obj.ohi) * cos(psi) / obj.m / r...
                        - obj.kh * psi * cos(psi) / obj.m / r...
                        - obj.g;
                    x(obj.dobi);
                    - obj.G * u(ControllerClass.tauhi) / obj.Ib;
                    x(obj.dohi);
                    obj.kh * psi / obj.Ih...
                        - obj.kh * x(obj.ohi) / obj.Ih...
                        + obj.G * u(ControllerClass.tauhi) / obj.Ih;
                    x(obj.doli);
                    - obj.kl * obj.rl^2 * x(obj.oli) / obj.G / obj.Il...
                        + r * obj.kl * obj.rl / obj.Il...
                        + obj.G * u(ControllerClass.tauli) / obj.Il;
                    obj.kx * obj.xtd / obj.bx...
                        - obj.kx * x(obj.xti) / obj.bx...
                        - obj.kl * obj.rl * x(obj.oli) * cos(psi) / obj.bx / obj.G...
                        + obj.kl * r * cos(psi) / obj.bx...
                        - obj.kh * x(obj.ohi) * cos(psi) / obj.bx / r...
                        + obj.kh * psi * cos(psi) / obj.bx / r;
                    0                                                                 ];            
        end
        
        function xdot = xrigid_yspringy_ground(obj, t, x)
            r   = obj.R(x);
            psi = obj.PSI(x);
            
            u = [0; 0];
            
            xdot = [x(obj.dxci);
                    obj.kl * obj.rl * x(obj.oli) * cos(PSI) / obj.G / obj.m...
                        - r * obj.kL * cos(psi) / obj.m...
                        + obj.kh * x(obj.ohi) * sin(psi) / obj.m / r...
                        - obj.kh * psi * sin(psi) / obj.m / r;
                    x(obj.dyci);
                    obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.G / obj.m...
                        - obj.kl * r * sin(psi) / obj.m...
                        + obj.kh * x(obj.ohi) * cos(psi) / obj.m / r...
                        - obj.kh * psi * cos(psi) / obj.m / r...
                        - obj.g;
                    x(obj.dobi);
                    - obj.G * u(ControllerClass.tauhi) / obj.Ib;
                    x(obj.dohi);
                    obj.kh * psi / obj.Ih...
                        - obj.kh * x(obj.ohi) / obj.Ih...
                        + obj.G * u(ControllerClass.tauhi) / obj.Ih;
                    x(obj.doli);
                    - obj.kl * obj.rl^2 * x(obj.oli) / obj.G / obj.Il...
                        + r * obj.kl * obj.rl / obj.Il...
                        + obj.G * u(ControllerClass.tauli) / obj.Il;
                    0;
                    0;                                                      ];          
        end
        
        function xdot = xspring_yrigid_ground(obj, t, x)
            r   = obj.R(x);
            psi = obj.PSI(x);
            
            u = [0; 0];
            
            xdot = [x(obj.dxci);
                    obj.kl * obj.rl * x(obj.oli) * cos(PSI) / obj.G / obj.m...
                        - r * obj.kL * cos(psi) / obj.m...
                        + obj.kh * x(obj.ohi) * sin(psi) / obj.m / r...
                        - obj.kh * psi * sin(psi) / obj.m / r;
                    x(obj.dyci);
                    obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.G / obj.m...
                        - obj.kl * r * sin(psi) / obj.m...
                        + obj.kh * x(obj.ohi) * cos(psi) / obj.m / r...
                        - obj.kh * psi * cos(psi) / obj.m / r...
                        - obj.g;
                    x(obj.dobi);
                    - obj.G * u(ControllerClass.tauhi) / obj.Ib;
                    x(obj.dohi);
                    obj.kh * psi / obj.Ih...
                        - obj.kh * x(obj.ohi) / obj.Ih...
                        + obj.G * u(ControllerClass.tauhi) / obj.Ih;
                    x(obj.doli);
                    - obj.kl * obj.rl^2 * x(obj.oli) / obj.G / obj.Il...
                        + r * obj.kl * obj.rl / obj.Il...
                        + obj.G * u(ControllerClass.tauli) / obj.Il;
                    0;
                    0;                                                      ];           
        end
        
        function xdot = xcompliant_yspringy_ground(obj, t, x)
            r   = obj.R(x);
            psi = obj.PSI(x);
            
            u = [0; 0];
            
            xdot = [x(obj.dxci);
                    obj.kl * obj.rl * x(obj.oli) * cos(PSI) / obj.G / obj.m...
                        - r * obj.kL * cos(psi) / obj.m...
                        + obj.kh * x(obj.ohi) * sin(psi) / obj.m / r...
                        - obj.kh * psi * sin(psi) / obj.m / r;
                    x(obj.dyci);
                    obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.G / obj.m...
                        - obj.kl * r * sin(psi) / obj.m...
                        + obj.kh * x(obj.ohi) * cos(psi) / obj.m / r...
                        - obj.kh * psi * cos(psi) / obj.m / r...
                        - obj.g;
                    x(obj.dobi);
                    - obj.G * u(ControllerClass.tauhi) / obj.Ib;
                    x(obj.dohi);
                    obj.kh * psi / obj.Ih...
                        - obj.kh * x(obj.ohi) / obj.Ih...
                        + obj.G * u(ControllerClass.tauhi) / obj.Ih;
                    x(obj.doli);
                    - obj.kl * obj.rl^2 * x(obj.oli) / obj.G / obj.Il...
                        + r * obj.kl * obj.rl / obj.Il...
                        + obj.G * u(ControllerClass.tauli) / obj.Il;
                    obj.kx * obj.xtd / obj.bx...
                        - obj.kx * x(obj.xti) / obj.bx...
                        - obj.kl * obj.rl * x(obj.oli) * cos(psi) / obj.bx / obj.G...
                        + obj.kl * r * cos(psi) / obj.bx...
                        - obj.kh * x(obj.ohi) * cos(psi) / obj.bx / r...
                        + obj.kh * psi * cos(psi) / obj.bx / r;
                    0                                                                     ];          
        end
        
        function xdot = xspringy_ycompliant_ground(obj, t, x)
            r   = obj.R(x);
            psi = obj.PSI(x);
            
            u = [0; 0];
            
            xdot = [x(obj.dxci);
                    obj.kl * obj.rl * x(obj.oli) * cos(PSI) / obj.G / obj.m...
                        - r * obj.kL * cos(psi) / obj.m...
                        + obj.kh * x(obj.ohi) * sin(psi) / obj.m / r...
                        - obj.kh * psi * sin(psi) / obj.m / r;
                    x(obj.dyci);
                    obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.G / obj.m...
                        - obj.kl * r * sin(psi) / obj.m...
                        + obj.kh * x(obj.ohi) * cos(psi) / obj.m / r...
                        - obj.kh * psi * cos(psi) / obj.m / r...
                        - obj.g;
                    x(obj.dobi);
                    - obj.G * u(ControllerClass.tauhi) / obj.Ib;
                    x(obj.dohi);
                    obj.kh * psi / obj.Ih...
                        - obj.kh * x(obj.ohi) / obj.Ih...
                        + obj.G * u(ControllerClass.tauhi) / obj.Ih;
                    x(obj.doli);
                    - obj.kl * obj.rl^2 * x(obj.oli) / obj.G / obj.Il...
                        + r * obj.kl * obj.rl / obj.Il...
                        + obj.G * u(ControllerClass.tauli) / obj.Il;
                    0;
                    obj.ky * obj.ytd / obj.by...
                        - obj.ky * x(obj.yti) / obj.by...
                        - obj.kl * obj.rl * x(obj.oli) * sin(psi) / obj.by...
                        + obj.kl * r * sin(psi) / obj.by...
                        - obj.kh * x(obj.ohi) * cos(psi) / obj.by / r...
                        + obj.kh * psi * cos(psi) / obj.by / r                              ];           
        end
        
        function [value, isterminal, direction] = stance_events(obj, t, x)
            r           = obj.R(x);
            rz          = obj.RZ(x);
            
            if ( x( obj.dyci ) < 0 )
                LO_event = 1;
            else
                LO_event =  r - rz;
            end
            
            value       = [ LO_event ; 1 ; x(obj.yci) - obj.gnd.ground_height(t, x(obj.xci), 0, inf) ];    
            isterminal  = [1 ; 0 ; 1 ];   
            direction   = [1 ; 0 ; 0 ];           
        end
        
        %------------------------------------------------------------------
        
        % Helpers
        
%         function toe = springy_ground_toe_position(obj, x)
%             r       = obj.R(x);
%             psi     = obj.PSI(x);
%             
%             toe     = [ obj.xtd...
%                     - obj.kl * obj.rl .* x(obj.oli, :) .* cos(psi) / obj.kx / obj.G...
%                     + obj.kl .* r .* cos(psi) / obj.kx...
%                     - obj.kh .* x(obj.ohi) .* cos(psi) / obj.kx...
%                     + obj.kh .* psi .* cos(psi) / obj.kx / r;
%                 obj.ytd...
%                     - obj.kl * obj.rl .* x(obj.oli) .* sin(psi) / obj.ky / obj.G...
%                     + obj.kl .* r .* sin(psi) / obj.ky...
%                     - obj.kh .* x(obj.ohi) .* cos(psi) / obj.ky / r...
%                     + obj.kh .* psi .* cos(psi) / obj.ky / r                         ];
%         end
        
    end
    
end